/*
 * @Author: pandaman
 * @Date: 2024-02-22 09:42:20
 * @LastEditors: 姚潘涛
 * @LastEditTime: 2024-11-11 17:49:55
 * @FilePath:
 * /xict_ws/src/xcmg-intelligent-cockpit-three/src/xict_cpp/src/xict_driver/task/gst_camera_driver_task.cc
 * @Description:
 *
 * Copyright (c) 2024 by pandaman, All Rights Reserved.
 */
#include "xict_driver/task/gst_camera_driver_task.h"
#include "xict_common/log/log.h"

namespace xict_driver {
    namespace task {
        CameraDriverTask::CameraDriverTask(
            const xict_driver::utils::CameraDriverConfig& _config,
            xict_driver::task::PeriodicTaskManager* _task_manager)
            : PeriodicTask(_task_manager, _config.period, _config.name),
              config_(_config) {
            image_topic_ = config_.topic;
        }

        CameraDriverTask::~CameraDriverTask() {
            if (cap_.isOpened()) {
                // 正确关闭管道
                cap_.release();
            }
        }

        void CameraDriverTask::Init() {
            ADEBUG_F("[CameraDriverTask] Init CameraDriverTask ");
            std::string gst_source;
            if (!config_.rtsp_enable) {
                gst_source_ = fmt::format(
                    "udpsrc port={} "
                    "caps=application/"
                    "x-rtp,media=(string)video,clock-rate=(int)90000,"
                    "encoding-name=(string)"
                    "H264,payload=(int)96 ! rtpjitterbuffer ! rtph264depay "
                    "! h264parse ! "
                    "queue ! avdec_h264 ! videoconvert ! video/x-raw ! "
                    "appsink sync=false",
                    config_.port);
            } else {
                gst_source_ = fmt::format(
                    "rtspsrc drop-on-latency=true location={} "
                    " latency=0 buffer-mode=auto ! rtpjitterbuffer ! "
                    "rtph264depay ! avdec_h264 ! videoconvert ! video/x-raw ! "
                    "appsink sync=false",
                    config_.rtsp_url);
            }
            cap_.open(gst_source_, cv::CAP_GSTREAMER);
            if (!cap_.isOpened()) {
                ADEBUG_F("Failed to open url: {}", gst_source_);
            } else {
                ADEBUG_F("Successed to open url: {}", gst_source_);
            }

            // 立即验证连接有效性
            cv::Mat test_frame;
            cap_ >> test_frame;
            if (test_frame.empty()) {
                ADEBUG_F("[CameraDriverTask] Initial frame check failed, "
                         "reconnecting...");
                Reconnect();   // 初始尝试失败后立即重连
            } else {
                ADEBUG_F("Successed to open url: {}", gst_source_);
            }
        }

        void CameraDriverTask::Run() {
            if (!cap_.isOpened()) {
                ADEBUG_F("[CameraDriverTask] Camera connection lost, "
                         "attempting to reconnect: {}",
                         image_topic_);
                Reconnect();
                return;
            }

            cap_ >> image_;

            if (!image_.empty()) {
                cb_img_(image_topic_, image_);
                consecutive_empty_frames_ = 0;
            } else {
                consecutive_empty_frames_++;
                ADEBUG_F(
                    "[CameraDriverTask] Failed to get image from camera: {}",
                    image_topic_);

                // 如果连续多次获取空帧，则重新建立连接
                if (consecutive_empty_frames_ >= MAX_EMPTY_FRAMES) {
                    ADEBUG_F("[CameraDriverTask] Too many empty frames, "
                             "reconnecting: {}",
                             image_topic_);
                    Reconnect();
                }
            }
        }

        void CameraDriverTask::Cleanup() {
            cap_.release();
            ADEBUG_F("[CameraDriverTask] Cleanup CameraDriverTask ");
        }

        void CameraDriverTask::RegisterDataPubCallback(
            const std::function<void(const std::string&, const cv::Mat&)>&
                _cb_data_pub) {
            cb_img_ = _cb_data_pub;
        }

        bool CameraDriverTask::Reconnect() {
            if (cap_.isOpened()) {
                cap_.release();   // 确保先释放现有连接
                std::this_thread::sleep_for(
                    std::chrono::milliseconds(1000));   // 短暂延时
            }

            bool success = false;
            try {
                success = cap_.open(gst_source_, cv::CAP_GSTREAMER);
            } catch (const cv::Exception& e) {
                ADEBUG_F(
                    "[CameraDriverTask] OpenCV exception during reconnect: {}",
                    e.what());
                return false;
            }

            if (success) {
                ADEBUG_F("[CameraDriverTask] Successfully reconnected to: {}",
                         image_topic_);
                consecutive_empty_frames_ = 0;

                // 验证连接是否真正建立
                cv::Mat test_frame;
                cap_ >> test_frame;
                if (test_frame.empty()) {
                    ADEBUG_F("[CameraDriverTask] Connection established but no "
                             "valid frame received");
                    cap_.release();
                    return false;
                }
            } else {
                ADEBUG_F("[CameraDriverTask] Failed to reconnect to: {}",
                         image_topic_);
            }

            return success;
        }
    }   // namespace task
}   // namespace xict_driver